/*******************************************************************************

                                CONCOSTS.C

  Title:        Source code for constraint cost functions 
  Author:       Hanna Szoke
  Date:         6/10/00  (separated from cost.c)
  Function:     This module contains functions used by Solve and
		called from e_funct() in cost.c, for calculating
		the various constraint cost functions.  These include:
			e_funct_np()
			e_funct_cs()
		and lower-level functions called by them.

*******************************************************************************/

#include "speden.h"	/* includes util.h, cellparams.h, symmetry.h, dims.h */
#include "speden_Np.h"		/* Np space arrays also in back.c, qshare.c */
#include "cost.h"		/* for cost function calculation */
#include "sp.h"



real e_funct_np(real *grad, real lamcoef, int k)

     /************************************************************
   	grad 	(returned) gradient array
	lamcoef  relative weight * con_coef
	k 	 target index 
     ************************************************************/
{
	int	j ;
	real	res_np = 0 ;
	real	*pwtp, *ptotp, *ptarg, *pgrad ; 

        ptotp = totp ;
	pgrad = grad ;
        pwtp = weightp + Nptotal*k ;
	ptarg = target + Nptotal*k ;

	for (j = 0; j < Nptotal; j++, pgrad++, pwtp++, ptotp++, ptarg++) {

             res_np += *pwtp * *pwtp * (*ptotp - *ptarg) * (*ptotp - *ptarg) ;
	     *pgrad += lamcoef * *pwtp * *pwtp * (*ptotp - *ptarg) ;

        }

        res_np *= lamcoef / 2. ;


        /* Reports */

        if (costfile_option) 
	     fprintf(fp_cost, ", Np: %10.4e", res_np) ;

	return (res_np) ;
}

real e_funct_cs(real *csol, real *grad, real lamcoef)
{
	void	do_cs_symmetrization(real *, real *) ;   /* in crysutil.c */
	int	j ;
	real	res_cs = 0 ;
	real	*pcsol, *pgrad ;
	real	*pave ;


	/* Compute the cs (crystallographic symmetry) cost function. */

	pave = extra ;
        do_cs_symmetrization(csol, pave) ;
	pcsol = csol ;

	for (j = 0; j < Nptotal; j++, pcsol++, pave++)
             res_cs += (*pcsol - *pave) * (*pcsol - *pave) ;
        res_cs *= lamcoef / 2. ;

      /* Add in the gradient contribution corresponding to the cs constraints */

	pgrad = grad ;
	pcsol = csol ;
	pave = extra ;

	for (j = 0; j < Nptotal; j++, pgrad++, pcsol++, pave++)
	     *pgrad +=  lamcoef * (*pcsol - *pave) ;

        /* Reports */

        if (costfile_option)
	     fprintf(fp_cost, ", Ncs: %10.4e", res_cs) ;
	   
	return (res_cs) ;
}
